/*
 * Matrix3.cpp
 *
 *  Created on: 2012/07/04
 *      Author: weihe
 */

#include "Matrix3.h"
#include <cmath>
#define PI       3.14159265358979323846

namespace geom {

template<class T>
Vector4<T> Matrix3::operator *(Vector4<T> v) {
	Vector4<T> result;
	T *ans = result.getArray();
	for (int i = 0; i < 3; i++) {
		ans[i] = m(i, 0) * v.x + m(i, 1) * v.y + m(i, 2) * v.z;
	}
	return result;
}

void RotateMatrix::rotateX(float rad) {
	float c = cosf(rad), s = sinf(rad);
	float *p = raw;

	*p++ = 1;
	*p++ = 0;
	*p++ = 0;

	*p++ = 0;
	*p++ = c;
	*p++ = -s;

	*p++ = 0;
	*p++ = s;
	*p++ = c;
}

void RotateMatrix::rotateY(float rad) {
	float c = cosf(rad), s = sinf(rad);
	float *p = raw;

	*p++ = c;
	*p++ = 0;
	*p++ = s;

	*p++ = 0;
	*p++ = 1;
	*p++ = 0;

	*p++ = -s;
	*p++ = 0;
	*p++ = c;
}

void RotateMatrix::rotateZ(float rad) {
	float c = cosf(rad), s = sinf(rad);
	float *p = raw;

	*p++ = c;
	*p++ = -s;
	*p++ = 0;

	*p++ = s;
	*p++ = c;
	*p++ = 0;

	*p++ = 0;
	*p++ = 0;
	*p++ = 1;
}

void RotateMatrix::rotateAngle(float rad, Vector4f axis) {
	float x = axis.x, y = axis.y, z = axis.z;
	float c = cosf(rad), s = sinf(rad);
	float C = 1 - c;
	float *p = raw;

	*p++ = x * x * C + c;
	*p++ = x * y * C - z * s;
	*p++ = x * z * C + y * s;

	*p++ = y * x * C + z * s;
	*p++ = y * y * C + c;
	*p++ = y * z * C - x * s;

	*p++ = z * x * C - y * s;
	*p++ = z * y * C + x * s;
	*p++ = z * z * C + c;
}

RotateMatrix::RotateMatrix(float angle, Vector4f axis) :
		Matrix3() {
	float th = angle / 180 * PI;
	if (&axis == &Vector4f::X_AXIS) {
		rotateX(th);
	} else if (&axis == &Vector4f::Y_AXIS) {
		rotateY(th);
	} else if (&axis == &Vector4f::Z_AXIS) {
		rotateZ(th);
	} else {
		rotateAngle(th, axis);
	}
}
}

/* namespace geom */
